#include "boundingbox.h"

// Included files for OpenGL Rendering
#ifdef __APPLE__
#include <OpenGL/gl.h>
#include <OpenGL/glu.h>
#include <GLUT/glut.h>
#else
#include <GL/gl.h>
#include <GL/glu.h>
#include <GL/glut.h>
#endif

#include "cloth.h"
#include "argparser.h"
#include <fstream>
#include "vectors.h"
#include <algorithm>
#include "glCanvas.h"
#include <cstring>
#include <utility>
#include "mesh.h"
#include "edge.h"
#include "vertex.h"

using namespace std;

float SPRING_INCREASE = 1;
float DIFF_INCREASE = 1;
float SHEAR_LENGTH_INCEREASE = 4;

vector<Vec3f> the_a;
vector<Vec3f> the_b;
vector<Vec3f> the_c;
vector<Vec3f> the_d;
//Mesh* GLCanvas::mesh;
// ================================================================================
// some helper drawing functions

void insertNormal(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3, const Vec3f &p4);
void DrawSpring(const Vec3f &a_o, const Vec3f &b_o, const Vec3f &a, const Vec3f &b, float correction);
Vec3f getNormal(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3, const Vec3f &p4);

// ================================================================================
// ================================================================================

Cloth::Cloth(ArgParser *_args, Mesh *_mesh) {
  args =_args;
  mesh = _mesh;
  _mesh->cloth = this;
  // open the file
  ifstream istr(args->cloth_file.c_str());
  assert (istr != NULL);
  string token;
  
  // read in the simulation parameters
  istr >> token >> k_structural; assert (token == "k_structural");  // (units == N/m)  (N = kg*m/s^2)
  istr >> token >> k_shear; assert (token == "k_shear");
  istr >> token >> k_bend; assert (token == "k_bend");
  istr >> token >> damping; assert (token == "damping");
  // NOTE: correction factor == .1, means springs shouldn't stretch more than 10%
  //       correction factor == 100, means don't do any correction
  istr >> token >> provot_structural_correction; assert (token == "provot_structural_correction");
  istr >> token >> provot_shear_correction; assert (token == "provot_shear_correction");

  // the cloth dimensions
  istr >> token >> nx >> ny; // (units == meters)
  assert (token == "m");
  assert (nx >= 2 && ny >= 2);

  // the corners of the cloth
  Vec3f a,b,c,d;
  istr >> token >> a; assert (token == "p");
  istr >> token >> b; assert (token == "p");
  istr >> token >> c; assert (token == "p");
  istr >> token >> d; assert (token == "p");

  // fabric weight  (units == kg/m^2)
  // denim ~300 g/m^2
  // silk ~70 g/m^2
  float fabric_weight;
  istr >> token >> fabric_weight; assert (token == "fabric_weight");
  float area = AreaOfTriangle(a,b,c) + AreaOfTriangle(a,c,d);

  // create the particles
  particles = new ClothParticle[nx*ny];
  float mass = area*fabric_weight / float(nx*ny);
  for (int i = 0; i < nx; i++) {
    float x = i/float(nx-1);
    Vec3f ab = (1-x)*a + x*b;
    Vec3f dc = (1-x)*d + x*c;
    for (int j = 0; j < ny; j++) {
      float y = j/float(ny-1);
      ClothParticle &p = getParticle(i,j);
      Vec3f abdc = (1-y)*ab + y*dc;
      p.setOriginalPosition(abdc);
      p.setPosition(abdc);
      p.setVelocity(Vec3f(0,0,0));
      p.setMass(mass);
      p.setFixed(false);
	  p.theI=i;
	  p.theJ=j;
	  p.completeCUT=false;
	  p.gravity=args->gravity;
    }
  }
  
  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////
  for(unsigned int i=0; i<nx; i++)
  {
	for(unsigned int j=0; j<ny; j++)
	{
		mesh->addVertex(getParticle(i,j).getPosition());
		int s = mesh->vertices.size()-1;
		getParticle(i,j).vert=mesh->getVertex(s); //->setPos(getParticle(i,j).getPosition());
		mesh->getVertex(s)->relationshipParticle= &getParticle(i,j);
		//cloth->getParticle(i,j).myVertex = mesh->getVertex(mesh->numVertices()-1);
		//mesh->getVertex(mesh->numVertices()-1);
	}
  }
  
  //cout<<"Number of Vertices: "<<mesh->numVertices()<<endl;
  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////
  int numv = mesh->numVertices();
  for (int i = 0; i < numv; ++i)
  {
      mesh->getVertex(i)->setOriginalPos(mesh->getVertex(i)->getPos());
      //std::cout << mesh->getVertex(i)->getOriginalPos() << std::endl;
  }
  /*
  Vertex *va = getVertex(a);
  Vertex *vb = getVertex(b);
  Edge *ab = getMeshEdge(va,vb);
  Edge *ba = getMeshEdge(vb,va);
  assert (ab != NULL);
  assert (ba != NULL);
  ab->setCrease(x);
  ba->setCrease(x);
  */
  
  
  // the fixed particles
  while (istr >> token) {
    assert (token == "f");
    int i,j;
    float x,y,z;
    istr >> i >> j >> x >> y >> z;
    ClothParticle &p = getParticle(i,j);
    p.setPosition(Vec3f(x,y,z));
    p.setFixed(true);
  }
  
  
  //Set spring relationships
  //
	for(int ii=0; ii<nx; ii++)
	{
		for(int jj=0; jj<ny; jj++)
		{
			ClothParticle &p = getParticle(ii,jj);
			p.cut=false;
			p.cut2=false;
			p.lastSpot=Vec3f(0,0,0);
			//cout<<ii<<" "<<jj<<":"<<endl;
			
			if(ii+1<nx) //struct -
			{
				ClothParticle &p = getParticle(ii,jj);
				ClothParticle &p2 = getParticle(ii+1,jj);
				p.struc.push_back(make_pair(ii+1,jj));//p2);
				p2.struc.push_back(make_pair(ii,jj));//p);
				
				if(jj+1<ny)
				{
					Vertex *va = mesh->getVertex(ii*nx+jj);
					Vertex *vb = mesh->getVertex((ii+1)*nx+jj);
					Vertex *vc = mesh->getVertex((ii+1)*nx+jj+1);
					/*va->setPos(va->getPos()-Vec3f(0.5,0.5,0.1));
					vb->setPos(vb->getPos()-Vec3f(0.5,0.5,0.1));
					vc->setPos(vc->getPos()-Vec3f(0.5,0.5,0.1));*/
					mesh->addTriangle(va,vb,vc); 
					
					Vertex *vd = mesh->getVertex(ii*nx+jj+1);
					//vd->setPos(vd->getPos()-Vec3f(0.5,0.5,0.1));
					mesh->addTriangle(va,vd,vc); 
					
					/*Edge *ab = mesh->getMeshEdge(va,vb);
					Edge *ba = mesh->getMeshEdge(vb,va);

					Edge *bc = mesh->getMeshEdge(vb,vc);
					Edge *cb = mesh->getMeshEdge(vc,vb);
					
					Edge *ca = mesh->getMeshEdge(vc,va);
					Edge *ac = mesh->getMeshEdge(va,vc);
					
					Edge *ad = mesh->getMeshEdge(va,vd);
					Edge *da = mesh->getMeshEdge(vd,va);
					
					Edge *dc = mesh->getMeshEdge(vd,vc);
					Edge *cd = mesh->getMeshEdge(vc,vd);*/
				}
				
				
				
			}
			if(jj+1<ny) //struct |
			{
				ClothParticle &p = getParticle(ii,jj);
				ClothParticle &p2 = getParticle(ii,jj+1);
				p.struc.push_back(make_pair(ii,jj+1));//p2);
				p2.struc.push_back(make_pair(ii,jj));
				//cout<<" "<<p2.getPosition()<<endl;
			}
			
			if( (jj+1<ny) && (ii+1<nx) ) //shear 
			{
				ClothParticle &p = getParticle(ii,jj);
				ClothParticle &p2 = getParticle(ii+1,jj+1);
				p.shear.push_back(make_pair(ii+1,jj+1));
				p2.shear.push_back(make_pair(ii,jj));
				//cout<<" "<<p2.getPosition()<<endl;
			}
			if( (jj-1>=0) && (ii+1<nx) ) //shear /
			{
				ClothParticle &p = getParticle(ii,jj);
				ClothParticle &p2 = getParticle(ii+1,jj-1);
				p.shear.push_back(make_pair(ii+1,jj-1));
				p2.shear.push_back(make_pair(ii,jj));
				//cout<<" "<<p2.getPosition()<<endl;
			}
			
			if(jj+2<ny) //bend |
			{
				ClothParticle &p = getParticle(ii,jj);
				ClothParticle &p2 = getParticle(ii,jj+2);
				p.bend.push_back(make_pair(ii,jj+2));
				p2.bend.push_back(make_pair(ii,jj));
				//cout<<" "<<p2.getPosition()<<endl;
			}
			if(ii+2<nx) //bend -
			{
				ClothParticle &p = getParticle(ii,jj);
				ClothParticle &p2 = getParticle(ii+2,jj);
				p.bend.push_back(make_pair(ii+2,jj));
				p2.bend.push_back(make_pair(ii,jj));
				//cout<<" "<<p2.getPosition()<<endl;
			}
			
		}
	}
	
	for(int ii=0; ii<nx; ii++)
	{
		for(int jj=0; jj<ny; jj++)
		{
			if(ii-1 >= 0 && jj+1 < ny)
			{
				Vertex *va = mesh->getVertex(ii*nx+jj);
				Vertex *vb = mesh->getVertex((ii-1)*nx+jj+1);
				Edge *ea = new Edge(va,vb);
				
				/*if(va->specialEdge)
				{
					cout<<"TEEHHEHE"<<endl;
					va->specialEdge2=ea;
					va->specialEdgePartner2=vb;
					
					//vb->specialEdge2=ea;
					//vb->specialEdgePartner2=va;
					
					vb->specialEdge=ea;
					vb->specialEdgePartner=va;
					
				}*/
				
				va->specialEdge=ea;
				va->specialEdgePartner=vb;
				//va->relationshipParticle->shear.push_back(make);
				
				vb->specialEdge2=ea;
				vb->specialEdgePartner2=va;
				
				
				
				//va->specialEdgePartner=vb;
				//vb->specialEdgePartner=va;
				
				mesh->edges[std::make_pair(va,vb)] = ea;
			}
		}
	}
					
  computeBoundingBox();
  
 
  
    for (int i = 0; i < nx-1; i++) {
      for (int j = 0; j < ny-1; j++) {
        	const Vec3f &curra = getParticle(i,j).getPosition();
        	const Vec3f &currb = getParticle(i,j+1).getPosition();
        	const Vec3f &currc = getParticle(i+1,j+1).getPosition();
        	const Vec3f &currd = getParticle(i+1,j).getPosition();
        	the_a.push_back(curra);
        	the_b.push_back(currb);
        	the_c.push_back(currc);
        	the_d.push_back(currd);
          }
    }
}

// ================================================================================




void Cloth::computeBoundingBox() {
  box = BoundingBox(getParticle(0,0).getPosition());
  for (int i = 0; i < nx; i++) {
    for (int j = 0; j < ny; j++) {
      box.Extend(getParticle(i,j).getPosition());
    }
  }
  
  int count=0;
  for(unsigned int h=0; h<nx; h++)//h<(nx*ny); h++)
  {
	//particles[h].box=BoundingBox(particles[h].getPosition());
	for(unsigned int k=0; k<ny; k++)
	{
		if( (h+1 < nx) && (k > 0) )
		{
			getParticle(h,k-1).box=BoundingBox(getParticle(h,k-1).getPosition());
			getParticle(h,k-1).box.Extend(getParticle(h+1,k).getPosition());
			getParticle(h+1,k).box=BoundingBox(getParticle(h+1,k).getPosition());
			getParticle(h+1,k).box.Extend(getParticle(h,k-1).getPosition());
			count++;
		}
		//box.extend
	}
  }
  //cout<<"count: "<<count<<" "<<nx*ny<<endl;
  
}

// ================================================================================
// ================================================================================
// ================================================================================

void Cloth::Paint(GLuint the_texture) const 
{

  // =====================================================================================
  // render the bounding box
  // =====================================================================================
  if (args->bounding_box) 
  {
    box.Paint();
	for(unsigned int i=0; i<(nx*ny); i++)
	{
		particles[i].box.Paint();
	}
  }

  // =====================================================================================
  // render the particles
  // =====================================================================================

  /*if (args->particles) {
    glDisable(GL_LIGHTING);
    glPointSize(3);
    glBegin(GL_POINTS);
    for (int i = 0; i < nx; i++) {
      for (int j = 0; j < ny; j++) {
		const ClothParticle &p = getParticle(i,j);
		const Vec3f &pos = p.getPosition();
		bool fixed = p.isFixed();
		if(p.completeCUT)
		{
			if (fixed) 
			  glColor3f(0,1,0);
			//else if (p.cut)
			  //glColor3f(1,1,1);
			else
			  glColor3f(0,0,0);	
			glVertex3f(pos.x(),pos.y(),pos.z());
		}
	  }
    }
    glEnd();
    glEnable(GL_LIGHTING);
  } */

  // =====================================================================================
  // render the velocity at each particle
  // =====================================================================================

  if (args->velocity) {
    glDisable(GL_LIGHTING);
    glLineWidth(2);
    glBegin(GL_LINES);
    glColor4f(10.0f,0.0f,0.0f,1.0f);
    for (int i = 0; i < nx; i++) {
      for (int j = 0; j < ny; j++) {
        const ClothParticle &p = getParticle(i,j);
        const Vec3f &pos = p.getPosition();
        const Vec3f &vel = p.getVelocity();
        Vec3f v = pos + vel;
        glVertex3f(pos.x(),pos.y(),pos.z());
        glVertex3f(v.x(),v.y(),v.z());
      }
    }
    glEnd();
    glEnable(GL_LIGHTING);
  } 

  // =====================================================================================
  // render the force at each particle
  // =====================================================================================

  if (args->force) {
    glDisable(GL_LIGHTING);
    glLineWidth(2);
    glBegin(GL_LINES);
    glColor3f(0,0,1);
    for (int i = 0; i < nx; i++) 
	{
      for (int j = 0; j < ny; j++) 
	  {
        const ClothParticle &p = getParticle(i,j);
		const Vec3f &pos = p.getPosition();
		Vec3f struc_force=Vec3f(0,0,0);
		Vec3f shear_force=Vec3f(0,0,0);
		Vec3f bend_force=Vec3f(0,0,0);
		for(int k=0; k<p.struc.size(); k++)
		{
			const ClothParticle &p2=getParticle(p.struc[k].first,p.struc[k].second);
			Vec3f a=p2.getPosition();
			Vec3f b=p.getPosition();
			Vec3f c=p2.getOriginalPosition();
			Vec3f d=p.getOriginalPosition();
			Vec3f s_pos=Vec3f( a.x()-b.x(), a.y()-b.y(), a.z()-b.z() );
			
			
			Vec3f d_pos=Vec3f( c.x()-d.x(), c.y()-d.y(), c.z()-d.z() );
			
			float dist= sqrt( (a.x()-b.x())*(a.x()-b.x()) + (a.y()-b.y())*(a.y()-b.y()) + (a.z()-b.z())*(a.z()-b.z()) );
			float dist2=sqrt( (c.x()-d.x())*(c.x()-d.x()) + (c.y()-d.y())*(c.y()-d.y()) + (c.z()-d.z())*(c.z()-d.z()) );
			
			float diff=(dist-dist2);
			
			Vec3f unit_vec=Vec3f( s_pos.x()/dist, s_pos.y()/dist, s_pos.z()/dist );
			unit_vec=unit_vec*diff;
			s_pos=unit_vec*k_structural;
			
			glVertex3f(pos.x(),pos.y(),pos.z());
			glVertex3f(pos.x()+s_pos.x(),pos.y()+s_pos.y(),pos.z()+s_pos.z());
			
		}
		for(int k=0; k<p.shear.size(); k++)
		{
			const ClothParticle &p2=getParticle(p.shear[k].first,p.shear[k].second);
			Vec3f a=p2.getPosition();
			Vec3f b=p.getPosition();
			Vec3f c=p2.getOriginalPosition();
			Vec3f d=p.getOriginalPosition();
			Vec3f s_pos=Vec3f( a.x()-b.x(), a.y()-b.y(), a.z()-b.z() );//-a; //Vec3f( a.x()-b.x(), a.y()-b.y(), a.z()-b.z() ) - Vec3f( c.x()-d.x(), c.y()-d.y(), c.z()-d.z() );
			//s_pos=s_pos*k_shear;//*p.getMass();
			
			Vec3f d_pos=Vec3f( c.x()-d.x(), c.y()-d.y(), c.z()-d.z() );
			
			float dist= sqrt( (a.x()-b.x())*(a.x()-b.x()) + (a.y()-b.y())*(a.y()-b.y()) + (a.z()-b.z())*(a.z()-b.z()) );
			float dist2=sqrt( (c.x()-d.x())*(c.x()-d.x()) + (c.y()-d.y())*(c.y()-d.y()) + (c.z()-d.z())*(c.z()-d.z()) );
			
			float diff=(dist-dist2);
			
			Vec3f unit_vec=Vec3f( s_pos.x()/dist, s_pos.y()/dist, s_pos.z()/dist );
			unit_vec=unit_vec*diff;
			s_pos=unit_vec*k_shear;
			
			glVertex3f(pos.x(),pos.y(),pos.z());
			glVertex3f(pos.x()+s_pos.x(),pos.y()+s_pos.y(),pos.z()+s_pos.z());
			
		}
		/*for(int k=0; k<p.bend.size(); k++)
		{
			const ClothParticle &p2=getParticle(p.bend[k].first,p.bend[k].second);
			Vec3f a=p2.getPosition();
			Vec3f b=p.getPosition();
			Vec3f c=p2.getOriginalPosition();
			Vec3f d=p.getOriginalPosition();
			Vec3f s_pos=Vec3f( a.x()-b.x(), a.y()-b.y(), a.z()-b.z() );//-a; //Vec3f( a.x()-b.x(), a.y()-b.y(), a.z()-b.z() ) - Vec3f( c.x()-d.x(), c.y()-d.y(), c.z()-d.z() );
			//s_pos=s_pos*k_bend;//*p.getMass();
			
			Vec3f d_pos=Vec3f( c.x()-d.x(), c.y()-d.y(), c.z()-d.z() );
			
			float dist= sqrt( (a.x()-b.x())*(a.x()-b.x()) + (a.y()-b.y())*(a.y()-b.y()) + (a.z()-b.z())*(a.z()-b.z()) );
			float dist2=sqrt( (c.x()-d.x())*(c.x()-d.x()) + (c.y()-d.y())*(c.y()-d.y()) + (c.z()-d.z())*(c.z()-d.z()) );
			
			float diff=(dist-dist2);
			
			Vec3f unit_vec=Vec3f( s_pos.x()/dist, s_pos.y()/dist, s_pos.z()/dist );
			unit_vec=unit_vec*diff;
			s_pos=unit_vec*k_bend;
			
			glVertex3f(pos.x(),pos.y(),pos.z());
			glVertex3f(pos.x()+s_pos.x(),pos.y()+s_pos.y(),pos.z()+s_pos.z());
			
		}*/

		Vec3f gravity_force=p.gravity*p.getMass();
		/*if( (i == (j+10)) && i>10)
		{
			if(j+10 < ny && i+1 < nx)
			{
			const ClothParticle &pp=getParticle(i,j);
			const ClothParticle &pp2=getParticle(i+1,j+10);
			Vec3f a=pp2.getPosition();
			Vec3f b=pp.getPosition();
			Vec3f slice_force=Vec3f( a.x()-b.x(), a.y()-b.y(), a.z()-b.z() );
			float dist= sqrt( (a.x()-b.x())*(a.x()-b.x()) + (a.y()-b.y())*(a.y()-b.y()) + (a.z()-b.z())*(a.z()-b.z()) );
			Vec3f unit_vec=Vec3f( slice_force.x()/dist, slice_force.y()/dist, slice_force.z()/dist );
			slice_force=unit_vec*1.01;
			gravity_force=gravity_force+slice_force;
			}
			
		}*/
		glVertex3f(pos.x(),pos.y(),pos.z());      
		glVertex3f(pos.x()+gravity_force.x(),pos.y()+gravity_force.y(),pos.z()+gravity_force.z());
	
      }
    }
    glEnd();
    glEnable(GL_LIGHTING);
  } 

  // =====================================================================================
  // render the cloth surface
  // =====================================================================================

  if (args->surface) {
  //std::cout << "surface" << std::endl;
  glEnable(GL_TEXTURE_2D);
  glBindTexture (GL_TEXTURE_2D, the_texture);
    glColor3f(1,1,1);
    glBegin(GL_QUADS);
    
    int count = 0;
    for (int i = 0; i < nx-1; i++) {
      for (int j = 0; j < ny-1; j++) {
	const Vec3f &a = getParticle(i,j).getPosition();
	const Vec3f &b = getParticle(i,j+1).getPosition();
	const Vec3f &c = getParticle(i+1,j+1).getPosition();
	const Vec3f &d = getParticle(i+1,j).getPosition();
	insertNormal(a,b,c,d);
	glTexCoord3f(the_a[count].x(),the_a[count].y(),the_a[count].z()); glVertex3f(a.x(),a.y(),a.z());
	glTexCoord3f(the_b[count].x(),the_b[count].y(),the_b[count].z()); glVertex3f(b.x(),b.y(),b.z());
	glTexCoord3f(the_c[count].x(),the_c[count].y(),the_c[count].z()); glVertex3f(c.x(),c.y(),c.z());
	glTexCoord3f(the_d[count].x(),the_d[count].y(),the_d[count].z()); glVertex3f(d.x(),d.y(),d.z());
	count += 1;
      }
    }
    glEnd();
    glDisable(GL_TEXTURE_2D);
  }

  // =====================================================================================
  // render the wireframe cloth (the structural and shear springs)
  // =====================================================================================

  if (args->wireframe) {
    glDisable(GL_LIGHTING);
    glLineWidth(1);
    glBegin(GL_LINES);
    for (int i = 0; i < nx-1; i++) {
      for (int j = 0; j < ny-1; j++) {
        const Vec3f &a_o = getParticle(i,j).getOriginalPosition();
        const Vec3f &b_o = getParticle(i,j+1).getOriginalPosition();
        const Vec3f &c_o = getParticle(i+1,j+1).getOriginalPosition();
        const Vec3f &d_o = getParticle(i+1,j).getOriginalPosition();        
        const Vec3f &a = getParticle(i,j).getPosition();
        const Vec3f &b = getParticle(i,j+1).getPosition();
        const Vec3f &c = getParticle(i+1,j+1).getPosition();
        const Vec3f &d = getParticle(i+1,j).getPosition();      
		//bool there=false;
		//for(unsigned int ii=0; i<getParticle(i,j).struc.size(); ii++)
		//	if(getParticle(i,j).struc[ii].first == i && getParticle(i,j).struc[ii].second == j+1)	
		DrawSpring(a_o,b_o,a,b, provot_structural_correction);
				
        DrawSpring(a_o,c_o,a,c, provot_shear_correction);
        DrawSpring(a_o,d_o,a,d, provot_structural_correction);
        DrawSpring(b_o,c_o,b,c, provot_structural_correction);
        DrawSpring(b_o,d_o,b,d, provot_shear_correction);
        DrawSpring(c_o,d_o,c,d, provot_structural_correction);
      }
    }
    glEnd();
    glEnable(GL_LIGHTING);
  }
}

// ================================================================================
// ================================================================================


void Cloth::MoveMesh()
{
	/*for(unsigned int i=0; i<nx; i++)
	{
		for(unsigned int j=0; j<ny; j++)
		{
			getParticle(i,j).vert->setPos(getParticle(i,j).getPosition());
			//mesh->getVertex(i*nx+j)->setPos(getParticle(i,j).getPosition());
		}
	}*/
	
	for(unsigned int i=0; i<mesh->numVertices(); i++)
	{
		Vertex *a=mesh->getVertex(i);
		if(a->relationshipParticle)
			a->setPos(a->relationshipParticle->getPosition());
	}
	
	/*for(unsigned int i=0; i<mesh->numVertices(); i++)
	{
		Vertex *a=mesh->getVertex(i);
		if(a->relationshipParticle)
		{
			if(a->counter % 100 == 0)//== 1000)
			{
				Vec3f diff = a->relationshipParticle->getPosition() - a->getPos();
				a->setPos(a->relationshipParticle->getPosition() + diff*.5);
				if(a->counter == 1000)
					a->counter=0;
			}
			else
				a->counter++;
		}
	}*/
	
}

void Cloth::Animate() 
{
	computeBoundingBox();
	
	Vec3f iPos, iVel, iAcc;
	Vec3f totalForce;
	Vec3f origL, l;
	int connectionCount = 0;
	int strucCount,shearCount = 0;
	//ClothParticle &connectionP = getParticle(0,0);
	float h = args->timestep;
	
	//checkBoundingBoxes(Vec3f &pos)
	
	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	//Now, move the cloth particles appropriately
	for (int i = 0; i < nx; i++) 
	{
		//if(pull_out==true) break;
		for (int j = 0; j < ny; j++) 
		{
			connectionCount = 0; strucCount = 0; shearCount = 0;
			ClothParticle &p = getParticle(i, j);
			p.forces.clear();
			totalForce = p.gravity * p.getMass(); //Start totalForce with gravity
			p.forces.push_back(p.gravity * p.getMass()); //Forces is used to draw forces acting on a particle later
		
			//if(pull_out==true) break;
			//ClothParticle &p = getParticle(i,j);
			//Vec3f pos=p.getPosition();
			
			/*Vec3f struc_force=Vec3f(0,0,0);
			Vec3f shear_force=Vec3f(0,0,0);
			Vec3f bend_force=Vec3f(0,0,0);*/
			if(!p.completeCUT)
			for(int k=0; k<p.struc.size(); k++)
			{	
				connectionCount++; strucCount++;
				ClothParticle &tempP = getParticle(p.struc[k].first,  p.struc[k].second);
				
				origL = tempP.getOriginalPosition() - p.getOriginalPosition();
				l = tempP.getPosition() - p.getPosition();
				//If stretched too far
				if (l.Length() > (1+provot_structural_correction) * origL.Length())
				{
					float diff = (l.Length() - (1+provot_structural_correction) * origL.Length() ) * DIFF_INCREASE;
					Vec3f tempPoint = l;
					Vec3f tempPoint2 = p.getPosition() - tempP.getPosition();
					tempPoint.Normalize();
					tempPoint2.Normalize();

					if(!p.isFixed() && !tempP.isFixed() && !tempP.completeCUT)
					{
						tempPoint = tempPoint * (diff / 2);
						tempPoint2 = tempPoint2 * (diff / 2);
						tempPoint += p.getPosition();
						tempPoint2 += tempP.getPosition();
						p.setPosition(tempPoint);
						tempP.setPosition(tempPoint2);
					}

					else if(!p.isFixed() && !tempP.completeCUT)
					{
						tempPoint = tempPoint * diff;
						tempPoint += p.getPosition();
						p.setPosition(tempPoint);
					}

					/*else if(l.Length() > (1+provot_structural_correction) * origL.Length() * SHEAR_LENGTH_INCEREASE)
					{
						cout<<"LEEMIT BREAKAH!"<<endl;
						p.struc.erase(p.struc.begin()+k);
						for(int kk=0; kk<tempP.struc.size(); kk++)
						{
							if(tempP.struc[kk].first == i && tempP.struc[kk].second == j)
							{
								tempP.struc.erase(tempP.struc.begin()+kk);
								break;
							}
						}
						
						//mesh->edges.erase(make_pair( p.vert, tempP.vert ) );
						//mesh->edges.erase(make_pair( tempP.vert, p.vert ) );
						//break;
					}*/
					
					else if (!tempP.isFixed() && !tempP.completeCUT)
					{
						tempPoint2 = tempPoint2 * diff;
						tempPoint2 += tempP.getPosition();
						tempP.setPosition(tempPoint2);
					}
					
					
					
				}
				
				//if pushed too close
				else if(l.Length() < (1-provot_structural_correction) * origL.Length())
				{
					float diff = ((1-provot_structural_correction) * origL.Length() - l.Length()) * DIFF_INCREASE ;
					diff = diff * -1;
					Vec3f tempPoint = l;
					Vec3f tempPoint2 = p.getPosition() - tempP.getPosition();
					tempPoint.Normalize();
					tempPoint2.Normalize();

					if(!p.isFixed() && !tempP.isFixed() && !tempP.completeCUT)
					{
						tempPoint = tempPoint * (diff / 2);
						tempPoint2 = tempPoint2 * (diff / 2);
						tempPoint += p.getPosition();
						tempPoint2 += tempP.getPosition();
						p.setPosition(tempPoint);
						tempP.setPosition(tempPoint2);
					}

					else if(!p.isFixed() && !tempP.completeCUT)
					{
						tempPoint = tempPoint * diff;
						tempPoint += p.getPosition();
						p.setPosition(tempPoint);
					}

					else if (!tempP.isFixed() )//&& !tempP.completeCUT)
					{
						tempPoint2 = tempPoint2 * diff;
						tempPoint2 += tempP.getPosition();
						tempP.setPosition(tempPoint2);
					}
				}
				
				l = tempP.getPosition() - p.getPosition();
				float len = origL.Length() - l.Length();
				len = -k_structural * len;
				l.Normalize();
				l = l * len * SPRING_INCREASE;
				if(!tempP.completeCUT)
				{
					totalForce += l;
					p.forces.push_back(l);
				}
				//if(tempP.completeCUT)
					//cout<<"LOL THIS IS WRONG."<<endl;
				
			}
			
			if(!p.completeCUT)
			for(int k=0; k<p.shear.size(); k++)
			{
				connectionCount++; shearCount++;
				ClothParticle &tempP = getParticle(p.shear[k].first,  p.shear[k].second);
				
				origL = tempP.getOriginalPosition() - p.getOriginalPosition();
				l = tempP.getPosition() - p.getPosition();

				if (l.Length() > (1+provot_shear_correction) * origL.Length())
				{
					float diff = (l.Length() - (1+provot_shear_correction) * origL.Length()) * DIFF_INCREASE;
					Vec3f tempPoint = l;
					Vec3f tempPoint2 = p.getPosition() - tempP.getPosition();
					tempPoint.Normalize();
					tempPoint2.Normalize();

					if(!p.isFixed() && !tempP.isFixed() && !tempP.completeCUT)
					{
						tempPoint = tempPoint * (diff / 2);
						tempPoint2 = tempPoint2 * (diff / 2);
						tempPoint += p.getPosition();
						tempPoint2 += tempP.getPosition();
						p.setPosition(tempPoint);
						tempP.setPosition(tempPoint2);
					}
					
					/*else if(l.Length() > (1+provot_shear_correction) * origL.Length() * SHEAR_LENGTH_INCEREASE)
					{
						cout<<"LEEMIT BREAKAH!"<<endl;
						p.shear.erase(p.shear.begin()+k);
						for(int kk=0; kk<tempP.shear.size(); kk++)
						{
							if(tempP.shear[kk].first == i && tempP.shear[kk].second == j)
							{
								tempP.shear.erase(tempP.shear.begin()+kk);
								break;
							}
						}
						
						//mesh->edges.erase(make_pair( p.vert, tempP.vert ) );
						//mesh->edges.erase(make_pair( tempP.vert, p.vert ) );
						//break;
					}*/

					else if(!p.isFixed() && !tempP.completeCUT)
					{
						tempPoint = tempPoint * diff;
						tempPoint += p.getPosition();
						p.setPosition(tempPoint);
					}

					
					else if (!tempP.isFixed() && !tempP.completeCUT)
					{
						tempPoint2 = tempPoint2 * diff;
						tempPoint2 += tempP.getPosition();
						tempP.setPosition(tempPoint2);
					}
					
				}

				else if(l.Length() < (1-provot_shear_correction) * origL.Length())
				{
					float diff = ((1-provot_shear_correction) * origL.Length() - l.Length()) * DIFF_INCREASE;
					diff = diff * -1;
					Vec3f tempPoint = l;
					Vec3f tempPoint2 = p.getPosition() - tempP.getPosition();
					tempPoint.Normalize();
					tempPoint2.Normalize();

					if(!p.isFixed() && !tempP.isFixed() && !tempP.completeCUT)
					{
						tempPoint = tempPoint * (diff / 2);
						tempPoint2 = tempPoint2 * (diff / 2);
						tempPoint += p.getPosition();
						tempPoint2 += tempP.getPosition();
						p.setPosition(tempPoint);
						tempP.setPosition(tempPoint2);
					}

					else if(!p.isFixed() && !tempP.completeCUT)
					{
						tempPoint = tempPoint * diff;
						tempPoint += p.getPosition();
						p.setPosition(tempPoint);
					}

					else if (!tempP.isFixed() )//&& !tempP.completeCUT)
					{
						tempPoint2 = tempPoint2 * diff;
						tempPoint2 += tempP.getPosition();
						tempP.setPosition(tempPoint2);
					}
				}

				l = tempP.getPosition() - p.getPosition();
				float len = origL.Length() - l.Length();
				len = -k_shear * len;
				l.Normalize();
				l = l * len * SPRING_INCREASE;
				if(!tempP.completeCUT)
				{
					totalForce += l;
					p.forces.push_back(l);
				}
			}
			/*for(int k=0; k<p.bend.size(); k++)
			{
				ClothParticle &tempP = getParticle(p.bend[k].first,  p.bend[k].second);
				origL = tempP.getOriginalPosition() - p.getOriginalPosition();
				l = tempP.getPosition() - p.getPosition();
				float len = origL.Length() - l.Length();
				len = -k_bend * len;
				l.Normalize();
				l = l * len * SPRING_INCREASE;
				if(!tempP.completeCUT)
				{
					totalForce += l;
					p.forces.push_back(l);
				}
			}*/
			
			//if(totalForce.y() <= p.gravity.y()*p.getMass()*20)
				//cout<<"lol totalForce.y: "<<totalForce.y()<<" other: "<<p.gravity.y()*p.getMass()*20<<" i: "<<i<<" j: "<<j<<endl;
			
			if(p.isFixed()==false) //&& !p.completeCUT)
			{
				iPos = p.getPosition();
				iVel = p.getVelocity();
	

				totalForce = totalForce - p.getVelocity() * damping;
				p.setPosition(iPos + (h * p.getVelocity()));
				p.setVelocity(iVel + (h * p.getAcceleration()));
				p.setAcceleration(totalForce * (1 / p.getMass()));	
				
				
			}
			
			if( (strucCount == 0 || shearCount == 0) && !p.completeCUT)
			{
				p.completeCUT = true;
			}
			
			/*if(connectionCount == 1 && !p.isFixed())
			{
				//connectionP;// = tempP;
				
				for(int k=0; k<p.shear.size(); k++)
				{
					ClothParticle &connectionP=getParticle(p.shear[k].first,p.shear[k].second);
					mesh->edges.erase(make_pair( p.vert, connectionP.vert ) );
					mesh->edges.erase(make_pair( connectionP.vert, p.vert ) );
				}
				/*Vertex *va = NULL;
				Vertex *vb = NULL;
				
				for(unsigned int i=0; i<mesh->numVertices(); i++)
				{
					Vertex *temp=mesh->getVertex(i);
					if(temp->relationshipParticle && temp->relationshipParticle == p)
					{
						va=temp;	
					}
					else if(temp->relationshipParticle && temp->relationshipParticle == connectionP)
					{
						vb=connectionP;
					}
				}
				
				if(a && b)
				{
					Edge *ab = getMeshEdge(va,vb);
					Edge *ba = getMeshEdge(vb,va);
				}
				
				p.struc.empty();
				p.shear.empty();
				mesh->cleanTriangles();
			}*/
			
		}
	}
	MoveMesh();
}


// ================================================================================
// ================================================================================
// some helper drawing functions
// ================================================================================

void DrawSpring(const Vec3f &a_o, const Vec3f &b_o, const Vec3f &a, const Vec3f &b, float correction) {
  Vec3f ab_o = b_o-a_o;
  Vec3f ab = b-a;
  float length_o = ab_o.Length(); // the original length
  float length = ab.Length();     // the current length
  if (length > (1+correction) * length_o ||
      length < (1-correction) * length_o) {
    // draw the spring in cyan if it's over-stretched
    glColor3f(0,1,1);
  } else {
    // otherwise draw it black
    glColor3f(0,0,0);
  }  
  glVertex3f(a.x(),a.y(),a.z());
  glVertex3f(b.x(),b.y(),b.z());
}

Vec3f getNormal(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3, const Vec3f &p4)
{
	// compute the normal of a non-planar quadrilateral (not well-defined)
  Vec3f v12 = p2 - p1;
  Vec3f v23 = p3 - p2;
  Vec3f v34 = p4 - p3;
  Vec3f v41 = p1 - p4;

  // compute normals at each corner and average them
  Vec3f normal1, normal2, normal3, normal4;
  Vec3f::Cross3(normal1,v41,v12);
  Vec3f::Cross3(normal2,v12,v23);
  Vec3f::Cross3(normal3,v23,v34);
  Vec3f::Cross3(normal4,v34,v41);
  Vec3f normal = normal1+normal2+normal3+normal4;

  normal.Normalize();
  return normal; 

}

void insertNormal(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3, const Vec3f &p4) {
  // compute the normal of a non-planar quadrilateral (not well-defined)
  Vec3f v12 = p2 - p1;
  Vec3f v23 = p3 - p2;
  Vec3f v34 = p4 - p3;
  Vec3f v41 = p1 - p4;

  // compute normals at each corner and average them
  Vec3f normal1, normal2, normal3, normal4;
  Vec3f::Cross3(normal1,v41,v12);
  Vec3f::Cross3(normal2,v12,v23);
  Vec3f::Cross3(normal3,v23,v34);
  Vec3f::Cross3(normal4,v34,v41);
  Vec3f normal = normal1+normal2+normal3+normal4;

  normal.Normalize();
  glNormal3f(normal.x(), normal.y(), normal.z());
  
}

void DrawForce(const Vec3f &p, const Vec3f &f) {
  Vec3f tmp = p+f;
  glVertex3f(p.x(),p.y(),p.z());
  glVertex3f(tmp.x(),tmp.y(),tmp.z());
}

// ================================================================================
// ================================================================================

